/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/motion_parameters.cpp
 * @Description  : 实现了运动参数的设置和管理，主要包括四个方面的参数：激光修正参数、速度参数、距离参数和控制参数。
 *                 通过从ROS参数服务器读取参数并设置到相应的类成员变量中，支持自动参数更新和不同等级的运动速度计算。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:09:17
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#include "motion_parameters.h"
using namespace std;
//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "DefaultReviseParams".
/**
 * @brief DefaultReviseParams 类的构造函数
 *
 * 该构造函数用于初始化 DefaultReviseParams 类的实例，并调用 SetDefaultParam 函数来设置默认参数。
 */
DefaultReviseParams::DefaultReviseParams() 
{
    SetDefaultParam();
}

/**
 * @brief 设置默认参数
 *
 * 该函数用于设置一些默认的参数。
 */
void DefaultReviseParams::SetDefaultParam() 
{
    SetSpecialLaserFParam();
    SetSpecialLaserBParam();
    SetSpecialLaserCurveParam();
    SetSpecialLaserSideParam();
}

/**
 * @brief 设置特殊激光参数
 *
 * 该函数用于设置特定激光参数组（LaserLineForward）的y轴修正值和角度修正值。
 *
 */
void DefaultReviseParams::SetSpecialLaserFParam() 
{
    int group = static_cast<int>(ParamSubType::LaserLineForward);
    laser_params_[group][0].modify_y_ = 5;
    laser_params_[group][0].modify_angle_ = 6;
    laser_params_[group][1].modify_y_ = 4;
    laser_params_[group][1].modify_angle_ = 5;
    laser_params_[group][2].modify_y_ = 3.5;
    laser_params_[group][2].modify_angle_ = 3.5;
    for (int j = 3; j < 5; j++) {
        laser_params_[group][j].modify_y_ = 0.5;
        laser_params_[group][j].modify_angle_ = 3;
    }
    for (int i = 5; i < 10; i++) {
        laser_params_[group][i].modify_y_ = 0.3;
        laser_params_[group][i].modify_angle_ = 1.2;
    }
}

/**
 * @brief 设置特殊激光B参数
 *
 * 该函数用于设置特定激光参数组（LaserLineBack）的激光参数。
 *
 */
void DefaultReviseParams::SetSpecialLaserBParam() {
    int group = static_cast<int>(ParamSubType::LaserLineBack);
    laser_params_[group][0].modify_y_ = 5;
    laser_params_[group][0].modify_angle_ = 6;
    laser_params_[group][1].modify_y_ = 4;
    laser_params_[group][1].modify_angle_ = 5;
    laser_params_[group][2].modify_y_ = 3.5;
    laser_params_[group][2].modify_angle_ = 3.5;
    for (int j = 3; j < 5; j++) {
        laser_params_[group][j].modify_y_ = 0.5;
        laser_params_[group][j].modify_angle_ = 3;
    }
    for (int i = 5; i < 10; i++) {
        laser_params_[group][i].modify_y_ = 0.3;
        laser_params_[group][i].modify_angle_ = 1.2;
    }
}

/**
 * @brief 设置特殊激光曲线参数
 *
 * 此函数用于设置特定激光曲线的参数，包括修改y值和修改角度。
 */
void DefaultReviseParams::SetSpecialLaserCurveParam() {
    int group = static_cast<int>(ParamSubType::LaserCurve);
    laser_params_[group][0].modify_y_ = 6;
    laser_params_[group][0].modify_angle_ = 8;
    laser_params_[group][1].modify_y_ = 5;
    laser_params_[group][1].modify_angle_ = 7;
    laser_params_[group][2].modify_y_ = 4;
    laser_params_[group][2].modify_angle_ = 6;
    laser_params_[group][3].modify_y_ = 3;
    laser_params_[group][3].modify_angle_ = 5;
    for (int j = 4; j < 6; j++) {
        laser_params_[group][j].modify_y_ = 1.5;
        laser_params_[group][j].modify_angle_ = 4;
    }
    laser_params_[group][6].modify_y_ = 1.3;
    laser_params_[group][6].modify_angle_ = 2.3;
    laser_params_[group][7].modify_y_ = 2;
    laser_params_[group][7].modify_angle_ = 3;
    laser_params_[group][8].modify_y_ = 2;
    laser_params_[group][8].modify_angle_ = 3.4;
    laser_params_[group][9].modify_y_ = 2;
    laser_params_[group][9].modify_angle_ = 3.8;
}

/**
 * @brief 设置特定激光侧移的参数
 *
 * 该函数用于设置特定激光侧移的参数，包括y轴偏移量和角度偏移量。
 *
 */
void DefaultReviseParams::SetSpecialLaserSideParam() {
    int group = static_cast<int>(ParamSubType::LaserSide);
    laser_params_[group][0].modify_y_ = 6;
    laser_params_[group][0].modify_angle_ = 8;
    laser_params_[group][1].modify_y_ = 5;
    laser_params_[group][1].modify_angle_ = 7;
    laser_params_[group][2].modify_y_ = 4;
    laser_params_[group][2].modify_angle_ = 6;
    laser_params_[group][3].modify_y_ = 3;
    laser_params_[group][3].modify_angle_ = 5;
    for (int j = 4; j < 6; j++) {
        laser_params_[group][j].modify_y_ = 1.5;
        laser_params_[group][j].modify_angle_ = 4;
    }
    laser_params_[group][6].modify_y_ = 1.3;
    laser_params_[group][6].modify_angle_ = 2.3;
    laser_params_[group][7].modify_y_ = 2;
    laser_params_[group][7].modify_angle_ = 3;
    laser_params_[group][8].modify_y_ = 2;
    laser_params_[group][8].modify_angle_ = 3.4;
    laser_params_[group][9].modify_y_ = 2;
    laser_params_[group][9].modify_angle_ = 3.8;
}

//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "CMotionParm".
short MotionParm::cur_level_ = 1;
float MotionParm::vel_step_ = 0.1f;
float MotionParm::up_acc_ = 0.3f;
float MotionParm::down_acc_ = 0.3f;
float MotionParm::up_low_acc_ = 0.1f;
float MotionParm::down_low_acc_ = 0.1f;
float MotionParm::transmit_vel_ = 0.0f;
LoadFlags MotionParm::load_flags_ = LoadFlags(); // 是否开启区分有货无货速度
float MotionParm::max_vel_ = 1.5f;
float MotionParm::min_adjust_radius_ = 5.0f;
float MotionParm::forward_stop_dis_ = 0.0f;
float MotionParm::backward_stop_dis_ = 0.0f;

int MotionParm::agv_model_ = static_cast<int>(AgvModel::model_diff);
DefaultReviseParams MotionParm::revise_params_ = DefaultReviseParams();
RefrainVelParams MotionParm::refrain_params_ = RefrainVelParams();
SpinParams MotionParm::spin_params_ = SpinParams();
CorrectParams MotionParm::correct_params_ = CorrectParams();
PidGroup MotionParm::pid_params_ = PidGroup();
LqrParamReader MotionParm::lqr_params_ = LqrParamReader();
ReadyToRunDelay MotionParm::read_to_run_delay_params_ = ReadyToRunDelay(); // 延迟启动参数
CurveVelParams MotionParm::curve_params_ = CurveVelParams();     // 曲线规划参数CMotionParm::CMotionParm()
SmoothParams MotionParm::smooth_params_ = SmoothParams();
SteerReadyDelayParams MotionParm::steer_ready_params_ = SteerReadyDelayParams();
/**
 * @brief 设置运动参数等级
 *
 * 设置当前运动参数的等级。等级必须在0到kLevels之间（包含0和kLevels）。
 *
 * @param level 等级值，类型为SHORT
 */
void MotionParm::SetLevel(short level) {
    if (level < LEVELS && level >= 0) {
        cur_level_ = level;
    }
}

/**
 * @brief 计算正常速度
 *
 * 根据给定的等级计算正常速度。
 *
 * @param level 等级，数据类型为SHORT
 * @return 计算得到的正常速度，数据类型为float
 */
float MotionParm::NormalVel(short level) {
    float vel = (level + 1) * vel_step_;
    return std::max(vel, MIN_NORMAL_VEL);
}

/**
 * @brief 根据给定的等级计算转弯速度
 *
 * 根据传入的等级参数，计算并返回转弯速度。
 *
 * @param level 等级，用于计算速度的参数
 * @return 计算得到的转弯速度
 */
float MotionParm::TurnVel(SHORT level) {
    float vel = NormalVel(level) * TURN_MOTION_RATIO;
    return std::max(vel, MIN_NORMAL_VEL);
}

/**
 * @brief 获取侧移速度
 *
 * 根据给定的等级计算侧移速度，并应用侧移比例，最后返回不小于最小侧移速度的速度值。
 *
 * @param level 等级，表示速度等级
 * @return float 返回经过侧移后的移动速度，速度值不小于最小侧移速度
 */
float MotionParm::ShiftVel(short level) {
    float vel = NormalVel(level) * SHIFT_MOTION_RATIO;
    return std::max(vel, MIN_SHIFT_VEL);
}

/**
 * @brief 计算旋转速度
 *
 * 根据给定的等级计算旋转速度。
 *
 * @param level 等级，类型为SHORT
 * @return 旋转速度，类型为float
 */
float MotionParm::SpinVel(short level) {
    float vel = NormalVel(level) * SPIN_MOTION_RATIO;
    return std::max(vel, MIN_SPIN_VEL);
}


// void CMotionParm::SetMaxVel(float fMaxVel)
//{
//     m_fMaxVel  = fMaxVel;
//     m_fVelStep = m_fMaxVel / LEVELS;
// }
/**
 * @brief 设置修正F参数
 *
 * @return 无返回值
 */
void MotionParm::SetReviseFParam() {
    int group = static_cast<int>(ParamSubType::LaserLineForward);

    const std::vector<std::pair<float, float>> default_values = {
        {4.0f, 5.0f},   // index 1
        {3.5f, 3.5f},   // index 2
        {0.5f, 3.0f},   // index 3
        {0.5f, 3.0f},   // index 4
        {0.3f, 1.2f},   // index 5
        {0.3f, 1.2f},   // index 6
        {0.3f, 1.2f},   // index 7
        {0.3f, 1.2f},   // index 8
        {0.3f, 1.1f}    // index 9
    };

    std::string group_head_y = "modify_line_forward.modify_y";
    std::string group_head_a = "modify_line_forward.modify_a";
    DefaultReviseParams& p = revise_params_;

    for (size_t i = 1; i <= 9; ++i) {
        p.laser_params_[group][i].modify_y_ = ROSNode()->get_parameter_or(
            group_head_y + std::to_string(i), 
            default_values[i-1].first
        );

        p.laser_params_[group][i].modify_angle_ = ROSNode()->get_parameter_or(
            group_head_a + std::to_string(i), 
            default_values[i-1].second
        );
    }

    DEBUG_OUT("F9:old:1.1 revise_params_.modify_angle:" << revise_params_.laser_params_[group][9].modify_angle_);
    DEBUG_OUT("SetReviseFParam:LaserLineForward: group is 0? =" << group);
    for (int i = 0; i < 9; i++) {
        float y = p.laser_params_[group][i].modify_y_;
        float a = p.laser_params_[group][i].modify_angle_;
        DEBUG_OUT("revise param:level i:" << i << ",y=" << y << ",a=" << a);
    }
}

/**
 * @brief 设置修正B参数
 *
 * @return 无返回值
 */
void MotionParm::SetReviseBParam() {
    int group = static_cast<int>(ParamSubType::LaserLineBack);
    std::string group_head_y = "modify_line_backward.modify_y";
    std::string group_head_a = "modify_line_backward.modify_a";
    DefaultReviseParams& p = revise_params_;
    const std::vector<std::pair<float, float>> default_values = {
        {5.0f, 6.0f},   // index 0
        {4.0f, 5.0f},   // index 1
        {3.5f, 3.5f},   // index 2
        {0.5f, 3.0f},   // index 3
        {0.5f, 3.0f},   // index 4
        {0.3f, 1.2f},   // index 5
        {0.3f, 1.2f},   // index 6
        {0.3f, 1.2f},   // index 7
        {0.3f, 1.2f},   // index 8
        {0.3f, 1.2f}    // index 9
    };

    for (size_t i = 0; i <= 9; ++i) {
        p.laser_params_[group][i].modify_y_ = ROSNode()->get_parameter_or(
            group_head_y + std::to_string(i), 
            default_values[i].first
        );
        p.laser_params_[group][i].modify_angle_ = ROSNode()->get_parameter_or(
            group_head_a + std::to_string(i), 
            default_values[i].second
        );
    }

    DEBUG_OUT("SetReviseFParam:LaserLineBack: group is 1? =" << group);
    for (int i = 0; i < 9; i++) {
        float y = p.laser_params_[group][i].modify_y_;
        float a = p.laser_params_[group][i].modify_angle_;
        DEBUG_OUT("revise param:level i:" << i << ",y=" << y << ",a=" << a);
    }
}

/**
 * @brief 设置曲线的参数
 *
 * @note 如果node_handle_为空，则打印错误信息并返回。
 */
void MotionParm::SetReviseCurveParam() {
    int group = static_cast<int>(ParamSubType::LaserCurve);
    std::string group_head_y = "modify_curve.modify_y";
    std::string group_head_a = "modify_curve.modify_a";
    DefaultReviseParams& p = revise_params_;
    const std::vector<std::pair<float, float>> default_values = {
        {6.0f, 8.0f},   // index 0
        {5.0f, 7.0f},   // index 1
        {4.0f, 6.0f},   // index 2
        {3.0f, 5.0f},   // index 3
        {1.5f, 4.0f},   // index 4
        {1.5f, 4.0f},   // index 5
        {1.3f, 2.3f},   // index 6
        {2.0f, 3.0f},   // index 7
        {2.0f, 3.4f},   // index 8
        {2.0f, 3.8f}    // index 9
    };

    // 使用循环设置所有参数
    for (size_t i = 0; i <= 9; ++i) {
        p.laser_params_[group][i].modify_y_ = ROSNode()->get_parameter_or(
            group_head_y + std::to_string(i), 
            default_values[i].first
        );
        p.laser_params_[group][i].modify_angle_ = ROSNode()->get_parameter_or(
            group_head_a + std::to_string(i), 
            default_values[i].second
        );
    }

    DEBUG_OUT("SetReviseFParam:LaserCurve: group is 2? =" << group);
    for (int i = 0; i < 9; i++) {
        float y = p.laser_params_[group][i].modify_y_;
        float a = p.laser_params_[group][i].modify_angle_;
        DEBUG_OUT("revise param:level i:" << i << ",y=" << y << ",a=" << a);
    }
}

/**
 * @brief 设置激光侧移正参数
 *
 * 此函数用于设置激光侧移参数。
 *
 * @return 无返回值
 */
void MotionParm::SetReviseSideParam() {
    int group = static_cast<int>(ParamSubType::LaserSide);
    std::string group_head_y = "/modify_side.modify_y";
    std::string group_head_a = "/modify_side.modify_a";
    DefaultReviseParams& p = revise_params_;
    const std::vector<std::pair<float, float>> default_values = {
        {6.0f, 8.0f},   // index 0
        {5.0f, 7.0f},   // index 1
        {4.0f, 6.0f},   // index 2
        {3.0f, 5.0f},   // index 3
        {1.5f, 4.0f},   // index 4
        {1.5f, 4.0f},   // index 5
        {1.3f, 2.3f},   // index 6
        {2.0f, 3.0f},   // index 7
        {2.0f, 3.4f},   // index 8
        {2.0f, 3.8f}    // index 9
    };

    // 使用循环设置所有参数
    for (size_t i = 0; i <= 9; ++i) {
        p.laser_params_[group][i].modify_y_ = ROSNode()->get_parameter_or(
            group_head_y + std::to_string(i), 
            default_values[i].first
        );
        p.laser_params_[group][i].modify_angle_ = ROSNode()->get_parameter_or(
            group_head_a + std::to_string(i), 
            default_values[i].second
        );
    }

    DEBUG_OUT("SetReviseFParam:LaserSide: group is 3? =" << group);
    for (int i = 0; i < 9; i++) {
        float y = p.laser_params_[group][i].modify_y_;
        float a = p.laser_params_[group][i].modify_angle_;
        DEBUG_OUT("revise param:level i:" << i << ",y=" << y << ",a=" << a);
    }
}

/**
 * @brief 设置速度参数
 *
 * 该函数用于从ROS参数服务器中读取速度参数，并设置到对应的成员变量中。
 */
void MotionParm::SetVelParam() {
    auto node = ROSNode();
    std::string vel_param = "velocity_param";
    up_acc_ = node->get_parameter_or(vel_param + ".acceleration", 0.3f);
    down_acc_ = node->get_parameter_or(vel_param + ".deceleration", 0.3f);
    up_low_acc_ = node->get_parameter_or(vel_param + ".up_low_acc", 0.1f);
    down_low_acc_ = node->get_parameter_or(vel_param + ".down_low_acc", 0.1f);
    transmit_vel_ = node->get_parameter_or(vel_param + ".transmit_vel", 0.0f); // 直线起步时，缓慢目标速度
    max_vel_ = node->get_parameter_or(vel_param + ".max_vel", 1.5f);
    min_adjust_radius_ = node->get_parameter_or(vel_param + ".min_adjust_radius", 5.0f);

    // 曲线规划参数
    auto& p = curve_params_;
    std::string load = ".load", unload = ".unload", line = ".line", bezier = ".bezier", spin = ".spin";
    std::string vmax = ".vmax", jmax = ".jmax", amax = ".amax";
    auto load_group = vel_param + load;
    auto unload_group = vel_param + unload;
    // 载货时，直线，曲线，自旋
    p.load.line.vmax = node->get_parameter_or(load_group + line + vmax, 2.0f);
    p.load.line.jmax = node->get_parameter_or(load_group + line + jmax, 4.0f);
    p.load.line.amax = node->get_parameter_or(load_group + line + amax, 1.0f);
    p.load.bezier.vmax = node->get_parameter_or(load_group + bezier + vmax, 2.0f);
    p.load.bezier.jmax = node->get_parameter_or(load_group + bezier + jmax, 4.0f);
    p.load.bezier.amax = node->get_parameter_or(load_group + bezier + amax, 1.0f);
    p.load.spin.vmax = node->get_parameter_or(load_group + spin + vmax, 2.0f);
    p.load.spin.jmax = node->get_parameter_or(load_group + spin + jmax, 4.0f);
    p.load.spin.amax = node->get_parameter_or(load_group + spin + amax, 1.0f);

    // 空载时，直线，曲线，自旋
    p.unload.line.vmax = node->get_parameter_or(unload_group + line + vmax, 2.0f);
    p.unload.line.jmax = node->get_parameter_or(unload_group + line + jmax, 4.0f);
    p.unload.line.amax = node->get_parameter_or(unload_group + line + amax, 1.0f);
    p.unload.bezier.vmax = node->get_parameter_or(unload_group + bezier + vmax, 2.0f);
    p.unload.bezier.jmax = node->get_parameter_or(unload_group + bezier + jmax, 4.0f);
    p.unload.bezier.amax = node->get_parameter_or(unload_group + bezier + amax, 1.0f);
    p.unload.spin.vmax = node->get_parameter_or(unload_group + spin + vmax, 2.0f);
    p.unload.spin.jmax = node->get_parameter_or(unload_group + spin + jmax, 4.0f);
    p.unload.spin.amax = node->get_parameter_or(unload_group + spin + amax, 1.0f);
    p.load.OutLog("load");
    p.unload.OutLog("unload");
    DEBUG_OUT("SetVelParam: up_acc_:"
        << up_acc_ << ",dn_acc_:" << down_acc_ << ",down_low_acc_:" << down_low_acc_
        << ",max_vel_:" << max_vel_
        << ",min_adjust_radius_:" << min_adjust_radius_
        << ",up_low_acc_:" << up_low_acc_
        << ",transmit_vel_:" << transmit_vel_);
}


void MotionParm::UpdateAllParam()
{
    SetVelParam();
    SetReviseFParam();
    SetReviseBParam();
    SetReviseCurveParam();
    SetReviseSideParam();
    SetDisParam();
    SetRefrainVelParam();
    SetSpinParam();
    SetCorrectParam();
    ReadOpenLoadVelParam();
    ReadPidParam();
    ReadLqrParam();
    ReadToRunParam();
}

void MotionParm::AutoUpdateAllParam() {
    if (EnableReadOnce()) {
        UpdateAllParam();
    }
}

/**
 * @brief 启用一次读取所有运动参数的功能
 *
 * 此函数检查是否启用了从ROS参数服务器一次读取所有运动参数的功能。
 *
 * @return 如果启用了该功能，则返回true；否则返回false。
 */
bool MotionParm::EnableReadOnce() {
    bool enable = ROSNode()->get_parameter_or("switch.cycle_read_all_motion_param", 0);
    DEBUG_OUT("EnableReadOnce: enable=" << enable);
    return enable;
}

/**
 * @brief 读取开启负载速度参数
 *
 * 从ROS参数服务器读取开启负载速度参数，并将其存储到类中。
 *
 * @return 如果成功读取参数并设置成功，则返回true；否则返回false。
 */
bool MotionParm::ReadOpenLoadVelParam() {
    bool open = ROSNode()->get_parameter_or("switch.open_load_vel", 0);
    load_flags_.open = open;
    DEBUG_OUT("ReadOpenLoadVelParam: enable=" << load_flags_.open);
    return load_flags_.open;
}

/**
 * @brief 设置距离参数
 *
 * 从ROS参数服务器读取距离参数，并设置给成员变量。
 *
 * 如果node_handle_为空，则输出错误信息并返回。
 * 从ROS参数服务器读取前进停止距离和后退停止距离，并将其设置为成员变量。
 * 将前进停止距离和后退停止距离的绝对值赋给成员变量。
 *
 * @return 无返回值
 */
void MotionParm::SetDisParam() {
    forward_stop_dis_ = ROSNode()->get_parameter_or("distance_param.forward_stop_dis", 0.0f);
    backward_stop_dis_ = ROSNode()->get_parameter_or("distance_param.backward_stop_dis", 0.0f);
    forward_stop_dis_ = fabs(forward_stop_dis_);
    backward_stop_dis_ = fabs(backward_stop_dis_);
    DEBUG_OUT("forward_stop_dis_:"
        << forward_stop_dis_ << ",backward_stop_dis_:" << backward_stop_dis_);
}

/**
 * @brief 设置限速参数
 *
 * 从ROS参数服务器读取限速参数，并保存到成员变量中。
 *
 * 如果节点句柄为空，则输出错误信息并返回。
 *
 * 从参数服务器读取的参数包括：
 * - `y_limit_bezier`：贝塞尔曲线y轴方向的限速
 * - `angle_limit_bezier`：贝塞尔曲线角度方向的限速
 * - `y_limit_line`：直线y轴方向的限速
 * - `angle_limit_line`：直线角度方向的限速
 *
 * 读取成功后，将参数输出到调试日志中。
 */
void MotionParm::SetRefrainVelParam() {
    auto node = ROSNode();
    refrain_params_.y_limit_bezier_ = node->get_parameter_or("refrain_vel_param.y_limit_bezier", 0.03f);
    refrain_params_.angle_limit_bezier_ = node->get_parameter_or("refrain_vel_param.angle_limit_bezier", 3.0f);
    refrain_params_.y_limit_line_ = node->get_parameter_or("refrain_vel_param.y_limit_line", 0.03f);
    refrain_params_.angle_limit_line_ = node->get_parameter_or("refrain_vel_param.angle_limit_line", 3.0f);
    DEBUG_OUT("SetRefrainVelParam:" << ",y_limit_bezier:" << refrain_params_.y_limit_bezier_
        << ",angle_limit_bezier:" << refrain_params_.angle_limit_bezier_
        << ",y_limit_line:" << refrain_params_.y_limit_line_
        << ",angle_limit_line:" << refrain_params_.angle_limit_line_);
}

/**
 * @brief 设置自旋参数
 *
 */
void MotionParm::SetSpinParam() {
    auto node = ROSNode();
    spin_params_.search_0_value_ = node->get_parameter_or("low_spin_param.search0Value", 0.08f);
    spin_params_.small_slope_ = node->get_parameter_or("low_spin_param.smallSlope", 0.05f);
    spin_params_.remain_angle_ = node->get_parameter_or("low_spin_param.remainAngle", 10.0f);
    DEBUG_OUT("SetSpinParam: search0Value:" << spin_params_.search_0_value_
        << ",smallSlope:" << spin_params_.small_slope_
        << ",remainAngle:" << spin_params_.remain_angle_);
}

/**
 * @brief 设置纠偏参数
 *
 * @note 如果成员变量node_handle_为空，则打印错误信息并返回。
 */
void MotionParm::SetCorrectParam() {
    auto node = ROSNode();
    correct_params_.low_ang_acc_ = node->get_parameter_or("correction_strength_param.lowSpeedAngAcc", 8.0f);
    correct_params_.mid_ang_acc_ = node->get_parameter_or("correction_strength_param.midSpeedAngAcc", 6.0f);
    correct_params_.high_ang_acc_ = node->get_parameter_or("correction_strength_param.highSpeedAngAcc", 4.0f);
    correct_params_.super_high_ang_acc_ = node->get_parameter_or("correction_strength_param.superHighSpeedAngAcc", 3.0f);
    DEBUG_OUT("SetCorrectParam: lowSpeedAngAcc:" << correct_params_.low_ang_acc_
       << ",midSpeedAngAcc:" << correct_params_.mid_ang_acc_
       << ",highSpeedAngAcc:" << correct_params_.high_ang_acc_
       << ",superHighSpeedAngAcc:" << correct_params_.super_high_ang_acc_);
}

/**
 * @brief 设置PID纠偏参数
 *
 * @note 如果成员变量node_handle_为空，则打印错误信息并返回。
 */
void MotionParm::ReadPidParam() {
    auto node = ROSNode();
    auto& p = pid_params_;
    static bool first = true;
    if (!first) {
        p.cycle_read = node->get_parameter_or("switch.cycle_read_pid_param", false);
        if (!p.cycle_read) {
            DEBUG_OUT_THROTTLE(node->get_logger(), 
                *(node->get_clock()), 200, "cycle Read pid param is false");
            return;
        }
    }
    node->get_parameter_or("switch.use_pid_algorithm", p.use, false);
    // node->get_parameter_or("base/controllers/twist_controller/wheel_separation", p.wheel_base, 1.0);
    std::string str_group_name = "pid_group.";
    std::string str_speed_name = "pid_low_speed.";
    p.low.speed = node->get_parameter_or(str_group_name + str_speed_name + "speed", 0.5f);
    p.low.pid_y.kp = node->get_parameter_or(str_group_name + str_speed_name + "P_Y", 1.0);
    p.low.pid_y.ki = node->get_parameter_or(str_group_name + str_speed_name + "I_Y", 0.0);
    p.low.pid_y.kd = node->get_parameter_or(str_group_name + str_speed_name + "D_Y", 0.0);
    p.low.pid_y.il = node->get_parameter_or(str_group_name + str_speed_name + "Il_Y", 0.0);
    p.low.pid_a.kp = node->get_parameter_or(str_group_name + str_speed_name + "P_A", 1.0);
    p.low.pid_a.ki = node->get_parameter_or(str_group_name + str_speed_name + "I_A", 0.0);
    p.low.pid_a.kd = node->get_parameter_or(str_group_name + str_speed_name + "D_A", 0.0);
    p.low.pid_a.il = node->get_parameter_or(str_group_name + str_speed_name + "Il_A", 0.0);

    str_speed_name = "pid_high_speed/";
    p.high.speed = node->get_parameter_or(str_group_name + str_speed_name + "speed", 0.5f);
    p.high.pid_y.kp = node->get_parameter_or(str_group_name + str_speed_name + "P_Y", 1.0);
    p.high.pid_y.ki = node->get_parameter_or(str_group_name + str_speed_name + "I_Y", 0.0);
    p.high.pid_y.kd = node->get_parameter_or(str_group_name + str_speed_name + "D_Y", 0.0);
    p.high.pid_y.il = node->get_parameter_or(str_group_name + str_speed_name + "Il_Y", 0.0);
    p.high.pid_a.kp = node->get_parameter_or(str_group_name + str_speed_name + "P_A", 1.0);
    p.high.pid_a.ki = node->get_parameter_or(str_group_name + str_speed_name + "I_A", 0.0);
    p.high.pid_a.kd = node->get_parameter_or(str_group_name + str_speed_name + "D_A", 0.0);
    p.high.pid_a.il = node->get_parameter_or(str_group_name + str_speed_name + "Il_A", 0.0);
    first = false;
    DEBUG_WARN_OUT("read_pid:wheel_base=" << p.wheel_base
                                          << ",low_speed=" << p.low.speed << ","
                                          << ",(Y):p=" << p.low.pid_y.kp
                                          << ",i=" << p.low.pid_y.ki
                                          << ",d=" << p.low.pid_y.kd
                                          << ",Il=" << p.low.pid_y.il
                                          << ",(A):p=" << p.low.pid_a.kp
                                          << ",i=" << p.low.pid_a.ki
                                          << ",d=" << p.low.pid_a.kd
                                          << ",Il=" << p.low.pid_a.il);
    DEBUG_WARN_OUT("read_pid:" << ",high_speed=" << p.high.speed
                               << ",(Y):p=" << p.high.pid_y.kp
                               << ",i=" << p.high.pid_y.ki
                               << ",d=" << p.high.pid_y.kd
                               << ",Il=" << p.high.pid_y.il
                               << ",(A):p=" << p.high.pid_a.kp
                               << ",i=" << p.high.pid_a.ki
                               << ",d=" << p.high.pid_a.kd
                               << ",Il=" << p.high.pid_a.il);
}

/**
 * @brief 设置LQR纠偏参数
 *
 * @note 如果成员变量node为空，则打印错误信息并返回。
 */
void MotionParm::ReadLqrParam() {
    auto node = ROSNode();
    auto& p = lqr_params_;
    static bool first = true;
    if (!first) {
        p.cycle_read = node->get_parameter_or("switch.cycle_read_lqr_param", false);
        if (!p.cycle_read) {
            DEBUG_OUT_THROTTLE(node->get_logger(), 
                *(node->get_clock()), 200, "cycle Read lqr param is false");
            return;
        }
    }
    p.use = node->get_parameter_or("switch.use_lqr_algorithm", false);
    p.lqr_threshold = node->get_parameter_or("switch.LQR_threshold", 0.9);

    std::string str_group_name = "lqr_group.";
    p.eps = node->get_parameter_or(str_group_name + "eps", 0.01);
    p.iterations = node->get_parameter_or(str_group_name + "iterations", 50);
    p.qx = node->get_parameter_or(str_group_name + "Qx", 1.0);
    p.qy = node->get_parameter_or(str_group_name + "Qy", 1.0);
    p.qtheta = node->get_parameter_or(str_group_name + "qtheta", 1.0);
    p.rv = node->get_parameter_or(str_group_name + "Rv", 1.0);
    p.rw = node->get_parameter_or(str_group_name + "Rw", 1.0);
    p.prepoint_dist = node->get_parameter_or(str_group_name + "length", 2.0);
    first = false;
    DEBUG_OUT("read_lqr:use_lqr_algorithm=" << p.use
                                            << ",lqr_threshold=" << p.lqr_threshold
                                            << ",eps=" << p.eps
                                            << ",iterations=" << p.iterations
                                            << ",qx=" << p.qx
                                            << ",qy=" << p.qy
                                            << ",qtheta=" << p.qtheta
                                            << ",rv=" << p.rv
                                            << ",rw=" << p.rw
                                            << ",length=" << p.prepoint_dist);
}

/**
 * @brief 读取空闲到启动参数
 *
 * @note 如果成员变量node_handle_为空，则打印错误信息并返回。
 */
void MotionParm::ReadToRunParam() {
    auto node = ROSNode();
    read_to_run_delay_params_.use = node->get_parameter_or("startToRunDelay.use", false);
    read_to_run_delay_params_.idle_time_ms = node->get_parameter_or("startToRunDelay.idleTimeMs", 1000);
    read_to_run_delay_params_.delay_time_ms = node->get_parameter_or("startToRunDelay.delayTimeMs", 3000);
    DEBUG_OUT("ReadToRunParam:use:" << read_to_run_delay_params_.use
        << ",idleTimeMs:" << read_to_run_delay_params_.idle_time_ms
        << ",delayTimeMs:" << read_to_run_delay_params_.delay_time_ms);
}

/**
 * @brief 读取AGV模型参数
 *
 * 从ROS参数服务器中读取AGV模型参数，并保存到成员变量中。
 *
 * 如果节点句柄为空，则输出错误信息并返回。
 *
 */
void MotionParm::ReadAgvModel() {
    int model = ROSNode()->get_parameter_or("/chassis.agv_model", static_cast<int>(AgvModel::model_diff));
    agv_model_ = model;
    DEBUG_OUT("ReadAgvModel:agv_model_:" << agv_model_);
}

/**
 * @brief 初始化参数
 *
 * 初始化MotionParm对象的参数，并确保node指向正确的ROS节点句柄。
 *
 * @param node_handle ROS节点句柄
 *
 * @return 如果初始化成功，返回true；否则返回false
 */
bool MotionParm::InitParam() {
    ReadAgvModel();
    UpdateAllParam();
    return true;
}